

// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include "sensor_msgs/msg/battery_state.hpp"  //电池状态
#include "modbus/modbus.h"

using namespace std::chrono_literals;

/* 发布电池电压信号 */

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class batteryPublisher : public rclcpp::Node
{
public:
  batteryPublisher()
  : Node("battery_publisher"), count_(0)
  {
    /*  第一件事情是 建立与电电池电压采样模块的链接 */
    ctx_voltage = NULL;        //初始化为未连接
    try_connect();

    publisher_ = this->create_publisher<sensor_msgs::msg::BatteryState>("battery", 10);
    timer_ = this->create_wall_timer(
      1s, std::bind(&batteryPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    if( ctx_voltage == NULL ){
        try_connect();
    }else{
        uint16_t value;   //得到电压
        if(modbus_read_registers(ctx_voltage,0x0258,1,(uint16_t *)&value) != -1){
            // 读取成功
            message.header.stamp = this->now();   //得到现在的时间
            message.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_GOOD;
            message.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
            message.voltage = value/100.f;    //不知道设置为0
        }else{
            message.header.stamp = this->now();   //得到现在的时间
            message.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
            message.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
        }
    }
    //RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    publisher_->publish(message);
  }
  /* 尝试链接 */
  void try_connect(void){
    ctx_voltage = modbus_new_rtu("/dev/usb_0", 9600, 'N', 8, 1);
    if (ctx_voltage == NULL) {
        //fprintf(stderr, "voltage device create error\n");
        message.header.stamp = this->now();   //得到现在的时间
        message.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
        message.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
        message.voltage = 0;    //不知道设置为0
    }else{
        modbus_set_slave(ctx_voltage, 0x1);
        if (modbus_connect(ctx_voltage) == -1) {
            fprintf(stderr, "voltage device connection failed: %s\n", modbus_strerror(errno));
            modbus_free(ctx_voltage);
            ctx_voltage = NULL;
            message.header.stamp = this->now();   //得到现在的时间
            message.power_supply_health = sensor_msgs::msg::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
            message.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
            message.voltage = 0;    //不知道设置为0
            /* 这里也是出错了 */
        }
        modbus_set_response_timeout(ctx_voltage,0,20000);  //200ms
        modbus_set_error_recovery(ctx_voltage,MODBUS_ERROR_RECOVERY_PROTOCOL);
    }
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr publisher_;
  size_t count_;
  modbus_t *ctx_voltage;   // 读取电池电压
  sensor_msgs::msg::BatteryState message;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<batteryPublisher>());
  rclcpp::shutdown();
  return 0;
}










